#include "zhongtong_can_adapter.h"
#include "sensor_utils.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>

using namespace sensor::radar;
using namespace sensor::utils;

ZTCanAdapter::ZTCanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs)
    : CanAdapter(devs, "zt")
{
    using namespace std::placeholders;
    message_unpack_table_ = {
        { ZT_VEHICLE_STATUS_FRAME_ID, std::bind(&ZTCanAdapter::onReceiveVehicleInfo, this, _1, _2, _3) },
        { ZT_CCVS_FRAME_ID, std::bind(&ZTCanAdapter::onReceiveCCVSInfo, this, _1, _2, _3) },
        { ZT_INDICATOR_FRAME_ID, std::bind(&ZTCanAdapter::onReceiveIndicatorInfo, this, _1, _2, _3) },
    };

    std::map<uint16_t, unsigned int> canIdMap = {
        { ZT_INDICATOR_FRAME_ID, CAN_EFF_MASK },
        { ZT_CCVS_FRAME_ID, CAN_EFF_MASK },
        { ZT_VEHICLE_STATUS_FRAME_ID, CAN_EFF_MASK },
    };
    can_devs_["can1"]->filter(canIdMap);
}

void ZTCanAdapter::onReceiveCanMessage(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    RCLCPP_INFO(logger_, "zt Rx:%s %04x %d %s", dev.c_str(), fid, buffer.readableBytes(),
        encodeToHexString(buffer.toStringPiece().as_string()).c_str());

    auto iter = message_unpack_table_.find(fid);
    if (iter == message_unpack_table_.end()) {
        // RCLCPP_WARN(logger_, "No Handle to unpack frame %04x", fid);
        return;
    }

    iter->second(dev, fid, buffer);
}
void ZTCanAdapter::onReceiveGnssMessage(const interface::msg::Gnss::SharedPtr msg)
{
}
void ZTCanAdapter::onReceiveInformationMessage(const interface::msg::CarInformation msg)
{
}

// 档位
void ZTCanAdapter::onReceiveVehicleInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    static std::unordered_map<int, int> gearsMap = {
        { 0, 0 },
        { 13, 7 },
        { 14, 8 },
    };
    struct zt_vehicle_status_t track;
    auto piece = buffer.toStringPiece();
    int retv = zt_vehicle_status_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    car_information_.gearstate = gearsMap[(track.current_gear)];
    RCLCPP_INFO(logger_, "[%04x]zt_vehicle_status_unpack return %d ,gearstate  %lf", fid, retv, car_information_.gearstate);
    body_information_pubs_->publish(car_information_);
}
// 车速
void ZTCanAdapter::onReceiveCCVSInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    struct zt_ccvs_t track;
    auto piece = buffer.toStringPiece();
    int retv = zt_ccvs_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    car_information_.speed = zt_ccvs_dash_vehicle_speed_decode(track.dash_vehicle_speed);
    body_information_pubs_->publish(car_information_);
    RCLCPP_INFO(logger_, "[%04x]zt_ccvs_unpack return %d , speed : %d", fid, retv, car_information_.speed);
}
// 转向
void ZTCanAdapter::onReceiveIndicatorInfo(const std::string&, int fid, muduo::net::Buffer& buffer)
{
    static std::unordered_map<int, int> indicatorMap = {
        { 0, 0 },
        { 1, 7 },
        { 2, 8 },
        { 3, 8 },
    };
    struct zt_indicator_t track;
    auto piece = buffer.toStringPiece();
    int retv = zt_indicator_unpack(&track, (const uint8_t*)(piece.data()), piece.size());
    switch (track.dash_indicator) {
    case 0:
        car_information_.leftturnlightst = 0;
        car_information_.rightturnlightst = 0;
        break;
    case 1:
        car_information_.rightturnlightst = 1;
        break;
    case 2:
        car_information_.leftturnlightst = 1;
        break;
    case 3:
        break;
    default:
        break;
    }
    body_information_pubs_->publish(car_information_);
    RCLCPP_INFO(logger_, "[%04x]zt_indicator_unpack return %d, leftturnlightst : %d, rightturnlightst : %d", fid, retv, car_information_.leftturnlightst, car_information_.rightturnlightst);
}
